/*
 * NaviMesh.cpp
 *
 *  Created on: 30/11/2011
 *      Author: Aeradon
 */

#include "../include/navimesh.hpp"
#include "../include/face.hpp"
#include "../include/symedge.hpp"

#ifdef _DEBUG
	#include <cstdio>
#endif

Navimesh::Navimesh(Triangulation *triangulation) :
		tri(triangulation)
{

}

void Navimesh::DestroyMesh()
{
	for (NaviNode* node : v_node)
	{
		if (node)
		{
			delete(node);
		}
	}
	v_node.clear();
}

void Navimesh::RebuildMesh()
{
	NaviNode* curNode;
	Face* sonFace;
	NaviNode* sonNode;
	SymEdge* sonEdge;

	std::vector<Face*>& v_face = tri->getVFace();

	DestroyMesh();

	for (std::vector<Face*>::iterator itr = ++(v_face.begin());
		 itr != v_face.end();
		 ++itr)
	{
		curNode = LocateOrCreateNode(*itr);

		(*itr)->node = curNode;

		// For each adjacent triangle along a non-constrained edge:
		for (int i = 0; i < 3; ++i )
		{
			sonEdge = (*itr)->getEdge(i);
			if (sonEdge->isConstrained || sonEdge->isBoundary)
			{
				curNode->n[i] = nullptr;
				continue;
			}

			sonFace = sonEdge->sym()->fac();

			sonNode = LocateOrCreateNode(sonFace);

			// Link it correctly with this one
			curNode->n[i] = sonNode;
		}
	}
}

Navimesh::~Navimesh()
{
	DestroyMesh();
}

NaviNode* Navimesh::CreateNode(Face* face)
{
	NaviNode* n = new NaviNode(face);
	v_node.push_back(n);
	n->vecIndex = v_node.size() -1;
	return n;
}

void Navimesh::RemoveNode(NaviNode* n)
{
#ifdef _DEBUG
	if (v_node.empty())
		printf("\nERROR: can't delete Node\n");
#endif

	v_node[n->vecIndex] = v_node.back();
	v_node[n->vecIndex]->vecIndex = n->vecIndex;
	v_node.pop_back();
	delete n;
}

NaviNode* Navimesh::LocateOrCreateNode(Face* face)
{
	// Search for the node representing this face in the vector
	NaviNode* node = LocateNode(face);

	// If it does not exist, insert it
	if (node == nullptr)
	{
		node = CreateNode(face);
	}

	return node;
}

NaviNode* Navimesh::LocateNode(Face* face)
{
	for (std::vector<NaviNode*>::iterator itr = v_node.begin();
		 itr != v_node.end();
		 ++itr)
	{
		if ((*itr)->fac() == face)
			return (*itr);
	}

	return nullptr;
}

///*
// * TODO Triangulation should do this dynamically, while building the mesh.
// */
//void Navimesh::CalculateSymEdgeNormals()
//{
//	std::vector<SymEdge*>&v_symEdge =  tri->getVSymEdge();
//	Vector2D normal, temp;
//
//	for (std::vector<SymEdge*>::iterator itr = v_symEdge.begin();
//		 itr != v_symEdge.end();
//		 ++itr)
//	{
//		temp = Vec2DNormalize(Vector2D((*itr)->vtx()->x, (*itr)->vtx()->y)
//				- Vector2D((*itr)->sym()->vtx()->x, (*itr)->sym()->vtx()->y));
//
//		normal.x = -temp.y;
//		normal.y = temp.x;
//
//		(*itr)->setNor(normal.x, normal.y);
//	}
//}

void Navimesh::ClearPathInfo()
{
	for (std::vector<NaviNode*>::iterator itr = v_node.begin();
		 itr != v_node.end();
		 ++itr)
	{
		(*itr)->g = 0.0;
		(*itr)->h = 0.0;
		(*itr)->f = 0.0;
		(*itr)->p = nullptr;
	}
}

Navimesh::T Navimesh::Heuristic(NaviNode* stt, NaviNode* dst)
{
	T c1 = (dst->X() - stt->X());
	T c2 = (dst->Y() - stt->Y());
	return (c1*c1 + c2*c2);
}

NaviNode* Navimesh::LowestFIn(std::list<NaviNode*>& l_node)
{
	NaviNode* lowestF_n = l_node.front();

	for (std::list<NaviNode*>::iterator itr = l_node.begin();
		 itr != l_node.end();
		 ++itr)
	{
		if(lowestF_n->f > (*itr)->f)
			lowestF_n = (*itr);
	}

	return lowestF_n;
}

bool Navimesh::IsInList(NaviNode* n, std::list<NaviNode*>& l_node)
{
	bool res = false;

	for (std::list<NaviNode*>::iterator itr = l_node.begin();
		 itr != l_node.end();
		 ++itr)
	{
		if(n == (*itr))
		{
			res = true;
			break;
		}
	}

	return res;
}

std::list<Point<Navimesh::T> > Navimesh::pathFind(T stt_x, T stt_y, T dst_x, T dst_y)
{
	// Vector to hold the path
	std::list<Point<T> > pathPoints;

	//	Find the node that represents the triangle that cointains stt point
	Point<T> stt_p; stt_p.setXY(stt_x, stt_y);
	Face* stt_f = tri->locatePointInFace(stt_x, stt_y);
	if (stt_f == nullptr)
	{
#ifdef _DEBUG
		printf("\n WARNING: Path start point not found in triangulation.\nReturning empty path.\n");
#endif
		return pathPoints;
	}
	NaviNode* stt_n = stt_f->node;

	//	Find the node that represents the triangle that cointains dst point
	Point<T> dst_p; dst_p.setXY(dst_x, dst_y);
	Face* dst_f = tri->locatePointInFace(dst_x, dst_y);
	if (dst_f == nullptr)
	{
#ifdef _DEBUG
		printf("\n WARNING: Path destiny point not found in triangulation.\nReturning empty path.\n");
#endif
		return pathPoints;
	}
	NaviNode* dst_n = dst_f->node;

	// If stt and dst are in the same node, the path is the dst point only
	if (stt_n == dst_n)
	{
		Point<T>* p = new Point<T>(dst_x, dst_y);
		pathPoints.push_back(*p);
		return pathPoints;
	}

	// ========================================================== Run A*
	// The map of navigated nodes.
	//std::list<NaviNode*> l_came_from;
	// The set of nodes already evaluated.
	std::list<NaviNode*> l_closed;
	// The set of tentative nodes to be evaluated, initially containing the start node
	std::list<NaviNode*> l_open;
	l_open.push_back(stt_n);

	// Cost from start along best known path.
	stt_n->g = 0.0;
	// Estimated total cost from this node to goal
	stt_n->h = Heuristic(stt_n, dst_n);
	// Estimated total cost from start to goal through this node.
	stt_n->f = stt_n->g + stt_n->h;

	NaviNode* x = nullptr;
	NaviNode* y = nullptr;
	T tentative_g_score = 0.0;
	bool tentative_is_better = false;

	while (!l_open.empty())
	{
		x = LowestFIn(l_open);

		if (x == dst_n)
		{
			pathPoints = BuildPointPath(dst_n);
			pathPoints.push_back(dst_p);
			return pathPoints;
		}

		l_open.remove(x);
		l_closed.push_back(x);

		for (int i = 0; i < 3; ++i)
		{
			y = x->n[i];

			if (y == nullptr)
				continue;

			if (IsInList(y, l_closed))
				continue;

			tentative_g_score = x->g + Heuristic(x,y);

			if (!IsInList(y, l_open))
			{
				l_open.push_back(y);
				tentative_is_better = true;
			}
			else if (tentative_g_score < y->g)
			{
				tentative_is_better = true;
			}
			else //tentative is not better
			{
				tentative_is_better = false;
			}

			if (tentative_is_better)
			{
				y->p = x;
				y->g = tentative_g_score;
				y->h = Heuristic(y,dst_n);
				y->f = y->g + y->h;
			}
		}
	}

	// No path found
#ifdef _DEBUG
	printf("\n WARNING: No path found.\nReturning empty path.\n");
#endif
	return pathPoints;
}

std::list<Navimesh::T> Navimesh::pathFindXY(T stt_x, T stt_y, T dst_x, T dst_y)
{
	std::list<T> l_xy;

	std::list<Point<T> > l_points = pathFind(stt_x, stt_y, dst_x, dst_y);

	for (std::list<Point<T> >::iterator itr = l_points.begin();
		 itr != l_points.end();
		 ++itr)
	{
		l_xy.push_back((itr)->x);
		l_xy.push_back((itr)->y);
	}

	return l_xy;
}

std::list<Point<Navimesh::T> > Navimesh::BuildPointPath(NaviNode* dst)
{
	std::list<Point<T> > pathPoints;
	NaviNode* n = dst;
	Point<T> nextPoint(0,0);

	// Create the path
	do
	{
		nextPoint.setXY(n->X(), n->Y());
		pathPoints.push_front(nextPoint);
		n = n->p;

	} while (n->p != nullptr);

	return pathPoints;
}

#ifdef _DEBUG
void Navimesh::printPath(std::list<Navimesh::T>& l_coord)
{
	printf("\n\nPrinting Path:\n\n");
	for (std::list<T>::iterator itr = l_coord.begin();
		 itr != l_coord.end();
		 ++itr)
	{
		printf("  (%d,",(int)(*itr));
		++itr;
		printf(" %d )  |",(int)(*itr));
	}
	printf("\n\n");
	fflush(stdout);
}

void Navimesh::printPath(std::list<Point<Navimesh::T> >& l_point)
{
	printf("\n\nPrinting Path:\n\n");
	for (std::list<Point<T> >::iterator itr = l_point.begin();
		 itr != l_point.end();
		 ++itr)
	{
		printf("  (%d, %d )  |",(int)(itr)->x,(int)(itr)->y);
	}
	printf("\n\n");
	fflush(stdout);
}
#endif
